import sympy
import math
from sympy import sin, cos, Matrix
from sympy.abc import rho, phi
a,b,c,x,y,z,a0,a1,a2 = sympy.symbols("a b c x y z a0 a1 a2")
a = -a1 * sin(y) - a2 * sin(y+z)
b = a0 * cos(x) + (a1 * cos(y) + a2 * cos(y+z)) * sin(x)
c = a0 * sin(x) - (a1 * cos(y) + a2 * cos(y+z)) * cos(x)
funcs = sympy.Matrix([a,b,c])
args = sympy.Matrix([x,y,z])
res = funcs.jacobian(args)
print(res)

# s12 = math.sin(theta[legIdx][1] / 180.0 * math.pi + theta[legIdx][2] / 180.0 * math.pi)
# c12 = math.cos(theta[legIdx][1] / 180.0 * math.pi + theta[legIdx][2] / 180.0 * math.pi)
# x = -a1 * s1 - a2 * s12
# y =  a0 * c0 + (a1 * c1 + a2 * c12) * s0
# z = a0 * s0 - (a1 * c1 + a2 * c12) * c0